package com.hitqz.robot.dispatch.netty.service.impl;

import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.cache.Cache;
import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
import com.hitqz.robot.api.business.entity.DispatchResourceEntity;
import com.hitqz.robot.api.business.entity.DispatchResourceRecordEntity;
import com.hitqz.robot.api.business.entity.RobotEntity;
import com.hitqz.robot.api.business.entity.RobotGroupEntity;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import com.hitqz.robot.biz.service.DispatchRecordService;
import com.hitqz.robot.biz.service.DispatchResourceRecordService;
import com.hitqz.robot.biz.service.DispatchResourceService;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.dispatch.netty.strategy.AlgorithmStrategy;
import com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil;
import com.hitqz.robot.dispatch.netty.util.JtsUtil;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Polygon;
import org.springframework.stereotype.Service;

import java.time.LocalDateTime;
import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.locks.ReentrantLock;
import java.util.stream.Collectors;

import static com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil.goFinalPos;

/**
 * @author xupkun
 * @date 2025/4/14
 */
@Service("LunduiAgvService")
@RequiredArgsConstructor
@Slf4j
public class LunduiAgvService implements AlgorithmStrategy {

    private final DispatchResourceService dispatchResourceService;

    private final DispatchResourceRecordService dispatchResourceRecordService;

    private final DispatchRecordService dispatchRecordService;

    private final RobotMapPosService robotMapPosService;

    /**
     *
     */
    private final Object lockC = new Object();

    /**
     *
     */
    private final Object lockB = new Object();

    /**
     *
     */
    private final Object lockA = new Object();
    private final Object lockD = new Object();
    private final Object lockF = new Object();

    private final Map<String, List<RobotEntity>> blockRobotMap = new ConcurrentHashMap<>();



    private String avoid = "AP37";

    private final Map<String, ReentrantLock> lockMap = new ConcurrentHashMap<>();

    private final Map<String, Boolean> sendMiddle = new ConcurrentHashMap<>();
    private ReentrantLock getLock(String robotSn) {
        return lockMap.computeIfAbsent(robotSn, k -> new ReentrantLock());
    }

    private static ThreadPoolExecutor threadPoolExecutor = new ThreadPoolExecutor(
            200,
            200,
            60,
            TimeUnit.SECONDS,
            new LinkedBlockingQueue<Runnable>(200),
            Executors.defaultThreadFactory(),
            new ThreadPoolExecutor.DiscardOldestPolicy());


    @Override
    public void changePath(RobotGroupEntity robotGroup, RobotApplyPathDto pathDto) {
        if (pathDto.getPathList().isEmpty()) {
            CheZhouAgvUtil.clearRobot(pathDto.getClientId());
        }
        //pathDto.getPathList().remove(0);
        // 去掉自身
        log.info("提交路径前往：{}---{}", pathDto.getClientId(), pathDto.getPathList().stream().map(RobotMapPosDto::getCode).
                collect(Collectors.joining("-")));
        CheZhouAgvUtil.sendPathFlag.put(pathDto.getClientId(), false);
        CheZhouAgvUtil.sendStopFlag.put(pathDto.getClientId(), false);
        sendMiddle.put(pathDto.getClientId(), false);
        CheZhouAgvUtil.submitPath(pathDto);
    }


    @Override
    public void calculate(int groupId, List<RobotEntity> robotEntityList) {
        Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
        LambdaQueryWrapper<DispatchResourceEntity> wrapper = Wrappers.lambdaQuery();
        wrapper.eq(DispatchResourceEntity::getMapCode, robotEntityList.get(0).getMapCode());
        if (robotEntityList.isEmpty()) {
            return;
        }
        try {
            for (RobotEntity robotEntity : robotEntityList) {
                RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
                RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
                if (robotClientSummaryDto == null) {
                    continue;
                }
                if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
                    continue;
                }
                String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
                String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
                if (StrUtil.isBlank(nearStation)) {
                    nearStation = lastArrivePos;
                }
                LinkedList<RobotMapPosDto> currentPathList = Optional.ofNullable(robotApplyPathDto).orElse(new RobotApplyPathDto()).getPathList();
                int pathListSize = 0;
                if (currentPathList != null && !currentPathList.isEmpty()) {
                    pathListSize = currentPathList.size();
                }
                int arrivePosIndex = -1;
                try {
                    // 如果路径列表较大，可以考虑使用哈希表加速查找
                    if (currentPathList != null && !currentPathList.isEmpty()) {
                        Map<String, Integer> pathCodeIndexMap = new HashMap<>();
                        for (int i = 0; i < pathListSize; i++) {
                            String code = currentPathList.get(i).getCode();
                            if (code != null) {
                                pathCodeIndexMap.put(code.toUpperCase(), i); // 转换为大写以支持忽略大小写的比较
                            }
                        }
                        // 快速查找匹配的路径点位
                        if (pathCodeIndexMap.containsKey(nearStation.toUpperCase())) {
                            arrivePosIndex = pathCodeIndexMap.get(nearStation.toUpperCase());
                        }
                    }

                } catch (NullPointerException e) {
                    log.error("解析当前位置时发生空指针异常: {}", e.getMessage(), e);
                    // 可以根据业务需求决定是否抛出异常或返回默认值
                } catch (Exception e) {
                    log.error("解析当前位置时发生未知异常: {}", e.getMessage(), e);
                    // 根据业务需求处理其他类型的异常
                }
                if (arrivePosIndex > 0) {
                    log.info("说明可能网络信号不太好，删除前面的元素");
                    // 使用 Iterator 安全地删除元素
                    Iterator<RobotMapPosDto> iterator = currentPathList.iterator();
                    int count = 0;
                    while (iterator.hasNext()) {
                        if (count < arrivePosIndex) {
                            iterator.remove(); // 安全删除
                        } else {
                            break; // 提前退出循环，避免不必要的迭代
                        }
                        count++;
                    }
                    arrivePosIndex = 0;
                }
                // 计算当前点到下一个路径的距离，然后提前进行申请
                RobotMapPosDto robotMapPosDto = null;
                if (robotApplyPathDto != null && robotApplyPathDto.getPathList() != null) {
                    LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
                    robotMapPosDto = pathList.get(0);
                    if (nearStation.equals(robotMapPosDto.getCode())) {
                        pathList.remove(0);
                        if (!pathList.isEmpty()) {
                            robotMapPosDto = pathList.get(0);
                        }
                    }
                }
                boolean applyNext = applyNext(robotEntity, nearStation, robotMapPosDto);
                processPath(robotEntity, nearStation, arrivePosIndex, currentPathList);
                if (!applyNext) {
                    // 由于失败所以查询
                    List<RobotEntity> blockRobot = blockRobotMap.get(robotEntity.getRobotSn());
                    // 跳过查询计算，由于两个车，离得最近的车直接去避让点
                    if (blockRobot != null && !blockRobot.isEmpty()) {
                        // 直接看本车在哪里
                        if (nearStation.equals("LM1") && !sendMiddle.get(robotEntity.getRobotSn())) {
                            sendMiddle.put(robotEntity.getRobotSn(), true);
                            RobotMapPosDto middlePos = new RobotMapPosDto();
                            middlePos.setCode(avoid);
                            log.info("{}前往中间点", robotEntity.getRobotSn());
                            goFinalPos(robotEntity.getRobotSn(), middlePos);
                        }
                    }

                }
            }
        } catch (Exception e) {
            log.error("获取资源信息失败", e);
        }

    }

    /**
     * 对路径数据进行处理
     */
    public void processPath(RobotEntity robotEntity, String nearStation, int arrivePosIndex, LinkedList<RobotMapPosDto> currentPathList) {
        lockC(lockC, () -> {
            if (arrivePosIndex == 0 && !currentPathList.isEmpty()) {
                logArrivalInfo(nearStation, currentPathList, arrivePosIndex, robotEntity);
                updateDispatchResourceRecords(robotEntity);
            }
        });
    }

    private void lockC(Object lock, Runnable task) {
        synchronized (lock) {
            try {
                task.run();
            } catch (Exception e) {
                log.error("路径处理过程中发生异常: {}", e.getMessage(), e);
            }
        }
    }

    private void logArrivalInfo(String nearStation, LinkedList<RobotMapPosDto> currentPathList, int arrivePosIndex, RobotEntity robotEntity) {
        String pathCodes = currentPathList.stream()
                .map(RobotMapPosDto::getCode)
                .collect(Collectors.joining("-"));
        //log.info("到达点位是 {} 计算之前的路径: {}", nearStation, pathCodes);
        RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
        //log.info("AGV执行剩余路径111111:" + currentPathList.stream()
        //        .map(RobotMapPosDto::getCode)
        //        .collect(Collectors.joining("-")));
        RobotMapPosDto robotMapPosDto = currentPathList.get(arrivePosIndex);
        if (nearStation.equals(robotMapPosDto.getCode())) {
            currentPathList.remove(arrivePosIndex);
            robotApplyPathDto.setPathList(currentPathList);
            CheZhouAgvUtil.robotApplyPathDtoMap.put(robotEntity.getRobotSn(), robotApplyPathDto);
            String remainingPathCodes = robotApplyPathDto.getPathList().stream()
                    .map(RobotMapPosDto::getCode)
                    .collect(Collectors.joining("-"));
            //log.info("AGV执行剩余路径22222: {}", remainingPathCodes);
            if (currentPathList.isEmpty()) {
                CheZhouAgvUtil.clearRobot(robotEntity.getRobotSn());
            }
        }

    }

    private void updateDispatchResourceRecords(RobotEntity robotEntity) {
        List<DispatchResourceRecordEntity> dispatchResourceRecords = dispatchResourceRecordService.list(
                Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                        .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                        .eq(DispatchResourceRecordEntity::getStatus, 1)
        );
        dispatchResourceRecords.forEach(record -> {
            record.setEndTime(LocalDateTime.now());
            record.setStatus(3);
            dispatchResourceRecordService.updateById(record);
        });
    }


    private void send2RobotNav(RobotEntity robotEntity, RobotMapPosDto dto) {
        // 参数校验
        try {
            // 检查是否需要发送路径
            if (!CheZhouAgvUtil.sendPathFlag.computeIfAbsent(robotEntity.getRobotSn(), key -> false)) {
                CheZhouAgvUtil.sendPathFlag.put(robotEntity.getRobotSn(), true); // 标记为已发送
                log.info("机器人 {} 下发可以前往位置 {}", robotEntity.getRobotSn(), dto.getCode());
                // 写入区域被占用
                // 调用外部方法发送最终位置
                goFinalPos(robotEntity.getRobotSn(), dto);
            }
        } catch (Exception e) {
            log.error("发送路径失败，机器人编号: {}, 目标位置: {}", robotEntity.getRobotSn(), dto.getCode(), e);
        }
    }


    /**
     * 修改导航状态
     *
     * @param status 导航状态
     */
    public static void changeNavStatus(RobotEntity robotEntity, int status, RobotMapPosDto robotMapPosDto) {
        // 确保 clientId 不为空
        synchronized (robotEntity.getRobotSn().intern()) {
            // 获取 sendPathFlag 的值并避免多次调用 get 方法
            Boolean sendPathFlagValue = CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn());
            if (sendPathFlagValue != null && sendPathFlagValue) {
                try {
                    Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
                    if (robotCache != null) {
                        RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
                        assert robotClientSummaryDto != null;
                        int taskStatus = robotClientSummaryDto.getStatus().getNavStatus();
                        if (taskStatus == 2 && status == 1) {
                            status = 3;
                            // 取消导航
                            SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(robotEntity.getRobotSn(), status));
                            TimeUnit.SECONDS.sleep(1);
                        }
                        if (taskStatus == 6 && status == 2) {
                            goFinalPos(robotEntity.getRobotSn(), robotMapPosDto);
                            TimeUnit.SECONDS.sleep(1);
                        }
                    }
                    if (status == 1) {
                        CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), true);

                    }
                    if (status == 2) {
                        CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), false);

                    }
                } catch (Exception e) {
                    // 捕获异常并记录详细日志
                    log.error("发布导航状态事件失败，clientId: {}, status: {}", robotEntity.getRobotSn(), status, e);
                }
            }
        }
    }


    public boolean applyNext(RobotEntity robotEntity, String nearStation, RobotMapPosDto robotMapPosDto) {
        synchronized (robotEntity.getRobotSn().intern()) {
            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
            Polygon polygon = robotPosPolygon.getIfPresent(robotEntity.getRobotSn());
            if (polygon == null) {
                return false;
            }
            // 如果下一个点是null 表示当前是不动的
            String pointCode;
            if (robotMapPosDto != null) {
                pointCode = robotMapPosDto.getCode();

            } else {
                pointCode = nearStation;
            }
            synchronized (lockB) {
                if (robotMapPosDto == null) {
                    // 没有下一个目标点
                    // 当前车是不动的写入当前被占用
                    DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
                            Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                    .eq(DispatchResourceRecordEntity::getAreaId, 0)
                                    .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                    .eq(DispatchResourceRecordEntity::getRemark, pointCode)
                                    .eq(DispatchResourceRecordEntity::getStatus, 1)
                                    .last("limit 1")
                    );
                    if (dispatchResourceRecordEntity == null) {
                        List<DispatchResourceEntity> list = findPosInArea(robotEntity, nearStation);
                        if (list.isEmpty()) {
                            dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
                            dispatchResourceRecordEntity.setRemark(pointCode);
                            dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
                            dispatchResourceRecordEntity.setStatus(1);
                            // 查询当前点是否在区域内
                            dispatchResourceRecordEntity.setAreaId(0);
                            dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
                            dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
                            dispatchResourceRecordService.save(dispatchResourceRecordEntity);
                        } else {
                            list.forEach(t -> {
                                synchronized (lockD) {
                                    DispatchResourceRecordEntity entity =
                                            dispatchResourceRecordService.getOne(
                                                    Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                                            .eq(DispatchResourceRecordEntity::getAreaId, t.getId())
                                                            .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                                            .eq(DispatchResourceRecordEntity::getRemark, pointCode)
                                                            .eq(DispatchResourceRecordEntity::getStatus, 1)
                                                            .last("limit 1")
                                            );
                                    if (entity == null) {
                                        entity = new DispatchResourceRecordEntity();
                                        entity.setRemark(pointCode);
                                        entity.setOccupyRobot(robotEntity.getRobotSn());
                                        entity.setStatus(1);
                                        // 查询当前点是否在区域内
                                        entity.setAreaId(t.getId());
                                        entity.setEndTime(LocalDateTime.now());
                                        entity.setStartTime(LocalDateTime.now());
                                        dispatchResourceRecordService.save(entity);
                                    }

                                }

                            });
                        }
                    }
                } else {
                    if (CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn()) != null && !CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn())) {
                        // 没有下发过导航
                        boolean applyArea = applyArea(robotEntity, pointCode);
                        if (applyArea) {
                            RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
                            if (robotApplyPathDto != null) {
                                RobotMapPosDto finalPos = robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size() - 1);
                                send2RobotNav(robotEntity, finalPos);
                            }
                        }
                    } else {
                        // 下发过
                        double distance = JtsUtil.getDistance(polygon, JtsUtil.getPoint(robotMapPosDto.getPosX(), robotMapPosDto.getPosY()));
                        if (distance <= 3.0) {
                            boolean applyResult = applyArea(robotEntity, pointCode);
                            if (!applyResult) {
                                changeNavStatus(robotEntity, 1, robotMapPosDto);
                            } else {
                                changeNavStatus(robotEntity, 2, robotMapPosDto);
                            }
                        }
                    }

                }
            }
        }

        return true;
    }


    /**
     * 申请区域
     *
     * @param robotEntity
     * @param nextPointCode
     * @return
     */
    private boolean applyArea(RobotEntity robotEntity, String nextPointCode) {
        ReentrantLock lock = getLock(robotEntity.getRobotSn());
        lock.lock();
        boolean writeNext = true;
        try {
            // 查询出来下一个点所在的区域
            List<DispatchResourceEntity> prepareArea = findPosInArea(robotEntity, nextPointCode);
            if (prepareArea.isEmpty()) {
                DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .eq(DispatchResourceRecordEntity::getAreaId, 0)
                                .ne(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getRemark, nextPointCode)
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .last("limit 1")
                );
                if (dispatchResourceRecordEntity != null) {
                    return writeNext;
                }
                dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .eq(DispatchResourceRecordEntity::getAreaId, 0)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getRemark, nextPointCode)
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .last("limit 1")
                );
                if (dispatchResourceRecordEntity == null) {
                    dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
                    dispatchResourceRecordEntity.setRemark(nextPointCode);
                    dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
                    dispatchResourceRecordEntity.setStatus(1);
                    // 查询当前点是否在区域内
                    dispatchResourceRecordEntity.setAreaId(0);
                    dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
                    dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
                    dispatchResourceRecordService.save(dispatchResourceRecordEntity);
                }
                dispatchResourceRecordService.update(
                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                .set(DispatchResourceRecordEntity::getStatus, 3)
                                .ne(DispatchResourceRecordEntity::getRemark, nextPointCode)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                );
            } else {
                // 查询准备申请的区域是否已经被占用
                List<DispatchResourceRecordEntity> existingArea = dispatchResourceRecordService.list(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .ne(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .in(DispatchResourceRecordEntity::getAreaId,
                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
                );
                if (existingArea != null && !existingArea.isEmpty()) {
                    return false;
                }
                // 查询是否存在当前的
                // 写入准备申请的区域
                prepareArea.forEach(t -> {
                    DispatchResourceRecordEntity entity = dispatchResourceRecordService.getOne(
                            Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                    .eq(DispatchResourceRecordEntity::getAreaId,
                                            t.getId())
                                    .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                    .eq(DispatchResourceRecordEntity::getStatus, 1));

                    if (entity == null) {
                        entity = new DispatchResourceRecordEntity();
                        entity.setRemark(nextPointCode);
                        entity.setOccupyRobot(robotEntity.getRobotSn());
                        entity.setStatus(1);
                        // 查询当前点是否在区域内
                        entity.setAreaId(t.getId());
                        entity.setEndTime(LocalDateTime.now());
                        entity.setStartTime(LocalDateTime.now());
                        dispatchResourceRecordService.save(entity);
                    }
                });

                dispatchResourceRecordService.update(
                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                .set(DispatchResourceRecordEntity::getStatus, 3)
                                .notIn(DispatchResourceRecordEntity::getAreaId,
                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                );
            }
        } catch (Exception e) {

        } finally {
            lock.unlock();
        }
        return writeNext;
    }


    /**
     * 查询申请的点位是不是在区域内
     *
     * @param robotEntity
     * @param posCode
     * @return
     */
    private List<DispatchResourceEntity> findPosInArea(RobotEntity robotEntity, String posCode) {
        List<DispatchResourceEntity> allResource = dispatchResourceService.list(
                Wrappers.<DispatchResourceEntity>lambdaQuery()
                        .eq(DispatchResourceEntity::getMapCode, robotEntity.getMapCode())
        );
        return allResource.stream().filter(item -> {
            String[] path = item.getPathList().split(",");
            for (String s : path) {
                if (s.equals(posCode)) {
                    return true;
                }
            }
            return false;
        }).toList();
    }


}
